#!/bin/bash
# 一键脚本部署mavros、fogros、rosbridge、mavproxy(本包)
# 尚未完成，不要使用
FCU_PORT=/dev/ttyACM0:921600
FCU_TPYE=apm

prepare_env() {
    source /opt/ros/noetic/setup.sh
    roscore
}

prepare_fogros() {
    prepare_env()
    cd $(pwd)/fogros2-sgc-main
    export SGC_CONFIG=talker.toml
    cargo run router
}

prepare_mavros() {
    prepare_env()
    roslaunch mavros apm.launch fcu_url:="$FCU_PORT"
}

get_service_status() {
    sudo chmod 777 /dev/ttyACM0
    SERVICE_NAME=$FCU_TPYE
 
    # 检查服务状态的命令
    SERVICE_STATUS_COMMAND="sudo systemctl status $SERVICE_NAME"
 
    # 使用grep获取服务状态行，过滤出Active:
    SERVICE_STATUS=$(service $SERVICE_NAME status)
 
    # 检查服务是否处于非活动状态
    if  echo "$SERVICE_STATUS" | grep -q "inactive (dead)"; then
        echo "$SERVICE_NAME 服务未运行，尝试重启..."
 
        # 尝试重启服务
        sudo systemctl restart apm.service
        # 再次检查服务状态
        SERVICE_STATUS=$(eval $SERVICE_STATUS_COMMAND | grep "active (running)")
    
        # 如果服务成功重启，打印状态
        if  echo "$SERVICE_STATUS" | grep -q "active (running)" ; then
            echo "$SERVICE_NAME 服务已成功重启。"
            sudo systemctl restart rosbridge.service
            sudo systemctl restart ros2control.service
        else
             "$SERVICE_NAME 服务重启失败。"
        fi

        else
            echo "$SERVICE_NAME 服务已运行。"
    fi
}

check_service(){
    while true
    do
        get_service_status
        sleep 5
    done
}

prepare_rosbridge() {
    sleep 5      
    . /opt/ros/noetic/setup.sh
    . /opt/ros/galactic/setup.sh
    ros2 run ros1_bridge dynamic_bridge --bridge-all-topics
}

prepare_mavproxy() {
    prepare_env
    source $(pwd)/../devel_isolated/mavproxy_ros1/setup.sh

    chmod +x $(pwd)/../src/mavproxy_ros1/scripts/*
    rosrun mavproxy_ros1 flight_cmd_parser.py
    rosrun mavproxy_ros1 flight_controller
}

start_mavproxy() {
    roslaunch mavproxy_apm.launch
}

# prepare_env
# prepare_mavros
# check_service
# prepare_rosbridge
prepare_mavproxy
start_mavproxy